#include "physics.h"
#include "..\common\def.h"
#include "..\common\util.h"
#include "..\engine\error.h"
#include "..\engine\engine.h"
#include "..\engine\object.h"
#include "..\physics\fieldactor.h"

#include "Physics\Collide\Dispatch\hkpCollisionDispatcher.h"
#include "Physics\Dynamics\World\hkpWorldCInfo.h"
#include "Common\Base\hkBase.h"
#include "Common\Base\System\hkBaseSystem.h"
#include "Physics\Collide\Dispatch\hkpAgentRegisterUtil.h"
#include "Common\Base\Memory\Memory\Pool\hkPoolMemory.h"
#include "Common\Base\Memory\hkThreadMemory.h"
#include "Common\Base\Memory\Memory\hkMemory.h"

#include <Common/Base/Reflection/hkClass.h>
#include <Common/Serialize/Util/hkLoader.h>
#include <Common/Serialize/Util/hkRootLevelContainer.h>
#include <Common/Base/Reflection/Registry/hkTypeInfoRegistry.h>
#include <Physics/Utilities/Serialize/hkpPhysicsData.h>
#include <Common/Base/Container/LocalArray/hkLocalBuffer.h>
#include <Common/SceneData/Scene/hkxScene.h>

#ifdef _DEBUG
#include "Common\Visualize\hkVisualDebugger.h"
#include "Physics\Utilities\VisualDebugger\hkpPhysicsContext.h"
#endif
#include <Common/Base/keycode.cxx>

#if !defined USING_HAVOK_PHYSICS
#error Physics is needed to build this demo. It is included in the common package for reference only.
#endif



// Classlists
#define INCLUDE_HAVOK_PHYSICS_CLASSES
#define HK_CLASSES_FILE <Common/Serialize/Classlist/hkClasses.h>
#include <Common/Serialize/Util/hkBuiltinTypeRegistry.cxx>

// Generate a custom list to trim memory requirements
#define HK_COMPAT_FILE <Common/Compat/hkCompatVersions.h>
#include <Common/Compat/hkCompat_None.cxx>


hkTransform TohkTransform(const Matrix4f& m)
{
	hkTransform out;
	out.set4x4ColumnMajor((float*)&m);	
	return out;
}
hkTransform ToEEhkTransform(float* pMat)
{
	//float or[9] = {pMat[0], pMat[3], pMat[6], 
	//				pMat[1], pMat[4], pMat[7],
	//				pMat[2], pMat[5], pMat[8]};
	//float cm[16] = {pMat[0], pMat[3], pMat[6], 0,
	//				pMat[1], pMat[4], pMat[7], 0,
	//				pMat[2], pMat[5], pMat[8], 0,
	//				pMat[9], pMat[10], pMat[11], 1};

	hkTransform out;
	hkRotation orrient;
	orrient.setRows(hkVector4(pMat[0], pMat[1], pMat[2], 0),
				 hkVector4(pMat[3], pMat[4], pMat[5], 0),
				 hkVector4(pMat[6], pMat[7], pMat[8], 0));
	orrient.transpose();
	out.setRotation(orrient);
	out.setTranslation(hkVector4(pMat[9], pMat[10], pMat[11], 1));

	//out.set4x4ColumnMajor(cm);
	/*
	out.setIdentity();
	out.setRows4(hkVector4(pMat[0], pMat[1], pMat[2], 0),
				 hkVector4(pMat[3], pMat[4], pMat[5], 0),
				 hkVector4(pMat[6], pMat[7], pMat[8], 0),
				 hkVector4(pMat[9], pMat[10], pMat[11], 1));
	out.setTranslation(hkVector4(pMat[9], pMat[10], pMat[11], 1));*/
	if(!out.isOk())
	{
		
	}
	return out;
}

Matrix4f ToMatrix4f(const hkTransform& m)
{
	Matrix4f out;
	m.get4x4ColumnMajor((float*)&out);
	return out;
}

Vector3f ToVector3f(const hkVector4& v)
{
	return Vector3f(v.getSimdAt(0),v.getSimdAt(1),v.getSimdAt(2));
}

hkVector4 TohkVector4(const Vector3f& v)
{
	return hkVector4(v.x,v.y,v.z);
}

/**************************************
 *
 **************************************/
CPhysics::CPhysics()
{
#ifdef _DEBUG
	pPhysicsContext	= NULL;
	mp_VisualDebug = NULL;
#endif
	mp_World	= NULL;
}

/**************************************
 *
 **************************************/
CPhysics::~CPhysics()
{
}

/**************************************
 * Returns the current physics scene of
 * the engine instance
 **************************************/
hkpWorld* CPhysics::GetScene()
{
	return mp_World;
}

/**************************************
 *Called on a per-frame basis to synchronize
 *physics objects with the application
 **************************************/
void CPhysics::Update(const float dt)
{
#ifdef _DEBUG
	if(ISNULL(mp_World))
	{
		EE_LOGERROR("No scene");
		return;
	}
#endif
	

#define PHYSICS_DT (1.0f / 25.0f)


	mp_World->stepDeltaTime(PHYSICS_DT);
#ifdef _DEBUG
#ifdef VISUAL_DEBUGGER
	mp_VisualDebug->step(PHYSICS_DT);
#endif
#endif
	for(unsigned int i = 0, j = m_Actors.Count(); i < j; i++)
		m_Actors[i]->Simulate();
}

/**************************************
 *Blocks threads that use simulation resources
 *so that the client application can modify/access
 *them safely
 **************************************/
void CPhysics::LockSimulation(void)
{
	if(mp_World)
		mp_World->lock();
}

/**************************************
 *Unblocks threads that use simulation resources
 *one the client application no longer requires access
 *to them for this frame
 **************************************/
void CPhysics::UnlockSimulation(void)
{
	if(mp_World)
		mp_World->unlock();

}

/**************************************
 *Releases all resources associated with
 *the physics instance
 **************************************/
HRESULT CPhysics::Release(void)
{
	try{

		if(mp_World)
		{
			delete mp_World;
			mp_World = NULL;
		}

#ifdef _DEBUG
#ifdef VISUAL_DEBUGGER
		mp_VisualDebug->removeReference();
		mp_VisualDebug = NULL;
#endif
#endif
		if(hkBaseSystem::quit() == hkResult::HK_FAILURE)
			throw EE_EXCEPTION("Failed to release physics instance.");

		return S_OK; 
	
	}catch(CError::CErrorData error) 
	{
		CError::CError(error, ERRORREPORT_LOG);
		return E_FAIL; 
	}
}

/**************************************
 *Callback for error events during simulation
 **************************************/
void CPhysics::OnError(const char* pMessage, void* pErrorObject)
{
	EE_LOGERROR(pMessage);
}

/**************************************
 *Allocates and initalizes all resources
 *required for the physics engine to function
 **************************************/
HRESULT CPhysics::Initailize()
{
	try{

		hkPoolMemory* memoryManager = new hkPoolMemory(); 
		hkThreadMemory* threadMemory = new hkThreadMemory(memoryManager); 
		
		if(hkBaseSystem::init(memoryManager, threadMemory, CPhysics::OnError) == hkResult::HK_FAILURE)
			throw EE_EXCEPTION("Failed to initialize physics.");
		memoryManager->removeReference();
		
		char* stackBuffer;
		{
			int stackSize = 0x100000;
			stackBuffer = hkAllocate<char>( stackSize, HK_MEMORY_CLASS_BASE);
			hkThreadMemory::getInstance().setStackArea( stackBuffer, stackSize);
		}

		hkpWorldCinfo info;
		info.m_gravity.set(0, -9.8f, 0);
		info.m_collisionTolerance = 0.001f;
		info.m_simulationType = info.SIMULATION_TYPE_CONTINUOUS;
		info.m_contactPointGeneration = hkpWorldCinfo::CONTACT_POINT_ACCEPT_ALWAYS;
		info.setBroadPhaseWorldSize(1000.0f);
		info.m_broadPhaseBorderBehaviour = hkpWorldCinfo::BROADPHASE_BORDER_FIX_ENTITY;

		mp_World = new hkpWorld( info );
		if(ISNULL(mp_World))
			throw EE_EXCEPTION("Failed to create physical world.");

		LockSimulation();

		{
			const hkReal characterTolerance = 0.001f;
			hkpCollisionQualityInfo& characterCollisionQuality = mp_World->m_collisionDispatcher->m_collisionQualityInfo[ hkpCollisionDispatcher::COLLISION_QUALITY_CHARACTER ];
			characterCollisionQuality.m_manifoldTimDistance = characterTolerance;
			characterCollisionQuality.m_createContact = characterTolerance;
			characterCollisionQuality.m_keepContact = characterTolerance;
			characterCollisionQuality.m_create4dContact = characterTolerance;
		}

		hkpAgentRegisterUtil::registerAllAgents( mp_World->getCollisionDispatcher() );			

#ifdef _DEBUG 
#ifdef VISUAL_DEBUGGER
		pPhysicsContext = new hkpPhysicsContext();
		pPhysicsContext->addWorld(mp_World);
		hkpPhysicsContext::registerAllPhysicsProcesses();

		hkArray<hkProcessContext*> contexts;
		contexts.pushBack(pPhysicsContext);
		mp_VisualDebug = new hkVisualDebugger(contexts);

		mp_VisualDebug->serve(25001);
#endif
#endif
		UnlockSimulation();
		return S_OK; 

	}catch(CError::CErrorData error)
	{
		CError::CError(error, ERRORREPORT_LOG);
		return E_FAIL; 
	}
}

/**************************************
 *Creates a physics character actor for 
 *external use by the client application
 *for characters
 **************************************/
HRESULT CPhysics::CreateCharacterActor(const string& filename, CCharacterActor** pOut, CActor::ActorData actorData, CObject *pParent)
{
	if(ISNULL(pOut))
		return E_FAIL;

	if(!filename.IsValid())
		return E_FAIL;

	CCharacterActor* pCharacterActor = EE_NEW CCharacterActor(this);
	
	if(ISNULL(pCharacterActor))
		return E_FAIL;

	if(FAILED(pCharacterActor->Create(filename, actorData,pParent)))
		return E_FAIL;

	m_Actors.Add(pCharacterActor);
	*pOut = pCharacterActor;
	return S_OK;

}

/**************************************
 *Creates a physical world by which other
 *physical actors will be contained and
 *constrained
 **************************************/
HRESULT CPhysics::CreateWorld(const string& filename, void** pHandle)
{
	string filepath;
	hkLoader* pLoader = NULL;
	hkRootLevelContainer* pContainer = NULL;
	hkxScene* pScene = NULL;
	hkpPhysicsData* pPhysics = NULL;
	HRESULT hr;

	do {
		
		filepath = CEngine::GetInstance()->FindFilepath(filename);
		pLoader = new hkLoader();

		if(!filepath.IsValid()) {
			hr = E_INVALIDARG;
			break;
		}
		
		pContainer = pLoader->load(filepath.c_str());
		if(ISNULL(pContainer)) {
			hr = E_UNEXPECTED;
			break;
		}

		pScene = reinterpret_cast<hkxScene*>( pContainer->findObjectByType( hkxSceneClass.getName() ));
		if(ISNULL(pScene)) {
			hr = E_UNEXPECTED;
			break;
		}

		pPhysics = reinterpret_cast<hkpPhysicsData*>( pContainer->findObjectByType( hkpPhysicsDataClass.getName() ));
		if(ISNULL(pPhysics)) {
			hr = E_UNEXPECTED;
			break;
		}

		const hkArray<hkpPhysicsSystem*>& psys = pPhysics->getPhysicsSystems();
		for (int i=0; i<psys.getSize(); i++)
		{
			hkpPhysicsSystem* system = psys[i];
			mp_World->addPhysicsSystem(system);
		}

		return S_OK;
	}while(false);

	if(pLoader) {
		pLoader->removeReference();
		pLoader = NULL;
	}

	if(pPhysics) {
		pPhysics->removeReference();
		pPhysics = NULL;
	}

	return hr;
}
	
/**************************************
 *Creates a physics actor for external
 *use by the client application
 **************************************/
HRESULT CPhysics::CreateActor(const string& filename, CActor** pOut, CActor::ActorData actorData, CObject *pParent)
{
	if(ISNULL(pOut))
		return E_FAIL;

	if(!filename.IsValid())
		return E_FAIL;

	CActor* pActor = EE_NEW CActor(this);
	
	if(ISNULL(pActor))
		return E_FAIL;

	if(FAILED(pActor->Create(filename, actorData,pParent)))
		return E_FAIL;

	m_Actors.Add(pActor);
	*pOut = pActor;
	return S_OK;
}

/**************************************
 *Creates a field actor that has no physical
 *presence but triggers when other actors intersect
 *it
 **************************************/
HRESULT CPhysics::CreateFieldActor(const string& filename, CActor** pOut, const Matrix4f& initalTransform, CObject *pParent) 
{
	
	if(ISNULL(pOut))
		return E_FAIL;

	if(!filename.IsValid())
		return E_FAIL;

	CFieldActor* pActor = EE_NEW CFieldActor(this);
	
	if(ISNULL(pActor))
		return E_FAIL;

	if(FAILED(pActor->Create(filename, initalTransform,pParent)))
		return E_FAIL;

	m_Actors.Add(pActor);
	*pOut = pActor;
	return S_OK;
}

/**************************************
 *Releases a physics actor that was created
 *for external use by the client application
 **************************************/
HRESULT CPhysics::ReleaseActor(CActor* pActor)
{
	if(ISNULL(pActor))
		return E_FAIL;

	if(FAILED(pActor->Release()))
		return E_FAIL;

	m_Actors.Remove(pActor);
	return S_OK;
}
